program spin_tracking
use madx_ptc_module
use pointer_lattice
implicit none

character*48 :: command_gino
type(probe) xs0,xs1,XST
type(probe_8) xs
type(layout), pointer :: als
real(dp) closed(6), x(6), theta0 ! 
INTEGER MF,mft,I,N,k,pos,no,kp,nturn
TYPE(FIBRE),POINTER:: P
TYPE(integration_node),POINTER:: t,t1,t2
type(internal_state) state
real(dp)  prec,cut,n_isf(3)  
logical first

type(c_damap) c_map,c_spin0,att,a_cs,as,a0,a1,a2,rota ,id_s
type(c_taylor) phase(3),nu_spin, fonction,fonction_FLOQUET
type(c_ray) cray
type(c_normal_form) c_n
TYPE(c_spinor) ISF,S_ISF,ISFoM,dISF
type(spinor) ISF_strobo   ! real spinor 

!-----------------------------------

first=.true. ; Lmax = 10.d0 ; use_info = .true.; prec=1.d-10;

spin_tune_def=-1     ! (1)
 
call ptc_ini_no_append

CALL  READ_AND_APPEND_VIRGIN_general(M_U,"ALS_FLAT_FAST.DAT")
write(6,*) "Making an node layout"
CALL MAKE_NODE_LAYOUT(m_U%start)

als=>m_U%start
p=>als%start

 
 
 
!!!!!  Make the lattice linear by zeroing all sextupoles !!!!!! 
!!!!!  Set all magnets spin to g=ag0 !!!!!! 
p=>als%start
do i=1,als%n
 IF(P%MAG%P%NMUL>=3) THEN
  CALL ADD(P,3,0,0.0000D0)
  if(first) then
   CALL ADD(P,2,0,0.2D0)
   first=.false.
  endif
 ENDIF
 p=>p%next
enddo

x(1:3)=1.d-4; x(5:6)=1.d-5; x(5)=1.d-2;
cut=4.d0
call MESS_UP_ALIGNMENT(als,x,cut)
x=0.d0
closed=0.d0

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

state=default0+nocavity0  

    CALL FIND_ORBIT(ALS,CLOSED,1,STATE,c_1d_5)
 
state=state+SPIN0

no=5
call init_all(STATE,no,0)

call alloc(c_map)
call alloc(c_n)
call alloc(c_spin0)
CALL ALLOC(ISF); call alloc(S_ISF); call alloc(ISFoM);call alloc(dISF);
call alloc(att,a_cs,as,a0,a1,a2,rota,id_s)
call alloc(phase)
call alloc(nu_spin,fonction,fonction_FLOQUET)
call kanalnummer(mf,"spin_result.txt")
call kanalnummer(mft,"spin_twiss.txt")

!!!! Polymorphic probe is created in the usual manner  
   XS0=CLOSED 
   ID_S=1
   XS=XS0+ID_S !  XS is probe_8,  XS0 is a probe with the closed orbit.  
   
!!!! get spin polymorphic probe after one turn   
CALL TRACK_PROBE(ALS,XS,STATE,FIBRE1=1)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 

! Copy probe_8 into a complex damap 
! because we use the complex package for normal forms
c_map=XS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
 

call c_normal(c_map,c_n,dospin=my_true)

write(6,*) c_n%tune(1:2), c_n%spin_tune

!att=c_n%A_t*c_n%As
att=c_n%As*c_n%A_t
 
! id_s is a rotation
id_s=att**(-1)*c_map*att

call c_full_canonize(id_s,a_cs,as,a0,a1,a2,rota,phase,nu_spin)

write(mft,*) " Fractional tune x"

call c_clean_taylor(phase(1),phase(1),prec)
call print(phase(1),mft)

 write(mft,*) " Fractional tune y"
call c_clean_taylor(phase(2),phase(2),prec)
call print(phase(2),mft)

 write(mft,*) " Fractional tune spin"
call c_clean_taylor(nu_spin,nu_spin,prec)
call print(nu_spin,mft)

phase(1)=0.d0 ;phase(2)=0.d0 ;nu_spin=0.d0;

att=c_n%As*c_n%A_t

call c_full_canonize(att,a_cs,as,a0,a1,a2,rota,phase,nu_spin)
 
phase(1)=0.d0 ;phase(2)=0.d0 ;nu_spin=0.d0;

   XS=XS0+A_CS 

p => als%start
 
do i=1,als%n
write(6,*) i,als%n
 CALL TRACK_PROBE(ALS,XS,STATE,FIBRE1=i,fibre2=i+1)

if((mod(i,300) == 0.or.i==als%n.or.i==1).or.no<=3) then
  xs0=xs
  att=XS
  call c_full_canonize(att,a_cs,as,a0,a1,a2,rota,phase,nu_spin)

   XS=XS0+A_CS 

  !!!!!!!!!!!!!!!   doing something  !!!!!!!!!!!!!!!!!

 fonction =(1.d0.cmono.1)**2   !  x**2

 call C_AVERAGE(fonction,A_CS,fonction_FLOQUET) 

 Write(mft,*) "Element ",p%mag%name

write(mft,*) " < x^2 > "
 call print(fonction_FLOQUET,mft)

write(mft,*) " phase x"

call c_clean_taylor(phase(1),phase(1),prec)
call print(phase(1),mft)

 write(mft,*) " phase y"
call c_clean_taylor(phase(2),phase(2),prec)
call print(phase(2),mft)

 write(mft,*) " phase spin"
call c_clean_taylor(nu_spin,nu_spin,prec)
call print(nu_spin,mft)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
endif

p=>p%next
enddo

!!!!  Exploring the ISF  !!!!

ISF=2 

ISF=c_n%As%s*ISF

Write(mft,*) " ISF "

call print(ISF,mft)


Write(mft,*) " Testing Barber's Equation S ISF = ISF o m "

S_ISF = c_map%s*ISF
ISFoM = ISF*c_map


Write(mft,*) "  |S ISF-  ISF o m|/ |S ISF| "

do i=1,3
dISF%v(i)=S_ISF%v(i)-ISFoM%v(i)
write(mft,*) i,full_abs(dISF%v(i)),full_abs(dISF%v(i))/full_abs(S_ISF%v(i))
enddo


x=0.d0
x(1)=0.001d0 ;x(3)=0.001d0 ;
xs1=closed+x
 
xst=0
cray%x=0.d0
cray%x(1:6)=x
do i=1,3
 n_isf(i) = ISF%v(i).o.cray
enddo


nturn=5000
kp=1000
call stroboscopic_average(als,xs1,xst,1,STATE,nturn,kp,ISF_strobo)

 
write(mft,*); 
write(mft,'(a19,4(1x,g20.13),a19,i4)') " ISF  for x(1:4) = ",x(1:4), " number of turns = ", nturn
write(mft,'(a24,3(1x,g20.13))') " Stroboscopic average ",ISF_strobo
write(mft,'(a24,3(1x,g20.13))') " From the normal form ",n_isf
write(mft,'(a4,20x,3(1x,g20.13))')" n0 ",  real(ISF%v(1).sub.'0'),real(ISF%v(2).sub.'0'),real(ISF%v(3).sub.'0')
 
close(mft)


 call ptc_end


end program spin_tracking


